#include <ltypes.h>
#include <lmacros.h>
#include <ltriMesh.h>
#include <lcollisionDetector.h>
#include <lcollisionResult.h>

LLIB_BEGIN

template <typename BVTree_t>
void geom::SceneObj<BVTree_t>::Init(std::vector<Vec3f>& vertexList, std::vector<Vec3i>& faceVertexIndexList)
{
    /* build up mesh */
    _mesh.SetModelData(vertexList, faceVertexIndexList);

    /* build up tree */
    _tree.Init(_mesh);

    /* set up collision flag */
    _collision = vector<Int32>(_mesh.GetFaceNum(), 0);
}

template <typename BVTree_t>
void geom::SceneObj<BVTree_t>::UpdateTree()
{
    BVTree_t::Iter* iter = _tree.CreateIter(BVTree_t::Iter::Postorder);
    BVTree_t::BVNode* node = iter->Next();
    while(node != NULL) {
        node->Update();
        node = iter->Next();
    }
    delete iter;
}

template <typename BVTree_t>
void geom::SceneObj<BVTree_t>::CollisionDetect( SceneObj<BVTree_t>& first, SceneObj<BVTree_t>& second )
{
    // the collision process
    BVTree_t* firstRoot = first._tree.GetRoot();
    BVTree_t* secondRoot = second._tree.GetRoot();
    // just ignore the collision result in non-self collision detect
    // collision result need to be created outside scene obj
    collision::CollisionDetector::CollisionDetect(firstRoot, secondRoot, NULL);
}

template <typename BVTree_t>
void geom::SceneObj<BVTree_t>::SelfCollisonDetect( SceneObj<BVTree_t>& obj )
{
    // the collision process
    collision::CollisionDetector::SelfCollisionDetect(obj._tree.GetRoot(), &obj._cr);
}

template <typename BVTree_t>
void geom::SceneObj<BVTree_t>::CollisionResponse( SceneObj& obj )
{
    std::fill(obj._collision.begin(),obj._collision.end(),0);
    obj._cr.Collect();

    if(SZ(obj._cr) > 2)
        Boolean pause = TRUE;

    FOR(r,0,SZ(obj._cr)) {
        FOR(c,0,SZ(obj._cr[r])) {
            MeshFace& tri = obj._cr[r][c];
            obj._collision[ tri.GetIndex() ] = r + 1;
        }
    }

    obj._cr.Reset();

}


LLIB_END
